/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <math.h>
#include <malloc.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <assert.h>
//#include <mex.h>
#include "linelib.h"


#define MAX_CORRELATION_JARI 0.7
#define ANGLE_REJECTION_LIMIT_JARI 0.3 // = 17 degrees

#ifndef LASER_MAX_DIST
#define LASER_MAX_DIST	81.0f
#endif

#define MAX_POINT_DIST 0.6

/**
 * 
 * @param x x-coordinates of points to be segmented    
 * @param y y-coordinates of points to be segmented
 * @param line output i.e. each column specifies one 
 *        segmented line with starting point, endpoint 
 *        and indexes of the starting and end point.
 * @param nump number of lines found
 * @param size size of x and y arrays.
 * @param Th Treshold distance from point to line.
 */
/* @param start = start index(maybe unnecessary, usually 0)*/
/* stop = stop index(maybe unnecessary, usually 361)*/
/* Th   = threshold, how large deviations from the  */
/*            line are accepted                     */
void split_and_merge(float *x, float *y, float *line, int *nump, int size, float Th)
{
  int i,j,k,end,ind,split,splitlist[1024],numtotest,splits[512];
  float a,b,c,norm,dx,dy,dist, meanDist;
  int start;
  int stop;
  int endOfLine;
  
  start=0;
  stop = size;
  
  /* split */
  splitlist[0]=stop-1;
  numtotest=1;
  j=1;
  splits[0]=start;
  while(numtotest>0)
  {
    end=splitlist[numtotest-1];
	if(end < 0 || start < 0) {
		(*nump)=0;
//		error(ERRS, "split_and_merge, error.");
		return;
	}	
    /* test split */
    /* calculate line parameters from start and end points of the segment */
    dx=x[start]-x[end];
    dy=y[start]-y[end];
    norm=sqrt(dx*dx+dy*dy);
    a=dy/norm;
    b=-dx/norm;
    c=-a*x[start]-b*y[start];

    /* check if the points between the two endpoints are close enough to the line*/ 
    split=0;
    for(i=start;i<end;i++)
    {
        
        dist=a*x[i]+b*y[i]+c;
        if(fabs(dist)>Th) /* if not then new split in the middle of this segment */
        {
            splitlist[numtotest]=(start+end)/2;
            numtotest++;
            split=1;
            break;
        }  
    }
    if(split==0) /* else test next segment */
    {
        splits[j]=end;
        j++;
        numtotest--;
        start=end;
    }
 }

 /* merge */
 start=splits[0];
 k=0;
 
 for(i=2;i<j;i++)
 {
    end=splits[i];
    ind=splits[i-1];
    dx=x[start]-x[end];
    dy=y[start]-y[end];
    norm=sqrt(dx*dx+dy*dy);
    a=dy/norm;
    b=-dx/norm;
    c=-a*x[start]-b*y[start];
    dist=a*x[ind]+b*y[ind]+c;
    if(fabs(dist)>Th)
    {
        line[k*SPLIT_AND_MERGE_D]=x[start];      //start
        line[k*SPLIT_AND_MERGE_D+1]=y[start];
        line[k*SPLIT_AND_MERGE_D+2]=x[ind];    //ind     
        line[k*SPLIT_AND_MERGE_D+3]=y[ind];
        line[k*SPLIT_AND_MERGE_D+4]=start;
        line[k*SPLIT_AND_MERGE_D+5]=ind;
        k++;
        start=ind;
    }
 }
 line[k*SPLIT_AND_MERGE_D]=x[start];
 line[k*SPLIT_AND_MERGE_D+1]=y[start];
 line[k*SPLIT_AND_MERGE_D+2]=x[end];
 line[k*SPLIT_AND_MERGE_D+3]=y[end];
 line[k*SPLIT_AND_MERGE_D+4]=start;
 line[k*SPLIT_AND_MERGE_D+5]=end;
 k++;

 (*nump)=k;
 //return;

 // Check extracted lines
 for(i=0;i<k;i++){
	 start = (int)line[i*SPLIT_AND_MERGE_D+4];
	 meanDist=0;
	 // Calculate average distance between points
	 for(j=(int)start;j<=(int)line[i*SPLIT_AND_MERGE_D+5];j++){	 
		meanDist += sqrt( (x[j]-x[j+1])*(x[j]-x[j+1]) + (y[j]-y[j+1])*(y[j]-y[j+1]));
	 }	
	 meanDist = meanDist/(line[i*SPLIT_AND_MERGE_D+5]-line[i*SPLIT_AND_MERGE_D+4]);
	 
	 endOfLine = (int)line[i*SPLIT_AND_MERGE_D+5];

	 for(j=(int)start;j<=endOfLine;j++){
		 //distance between neighbour points 
		 dist = sqrt( (x[j]-x[j+1])*(x[j]-x[j+1]) + (y[j]-y[j+1])*(y[j]-y[j+1]));
		 // if too large reject
		 //if(dist> 10.0 *Th){
		 if(dist> 2.0 * meanDist || dist>MAX_POINT_DIST){
			 if( (j-(int)line[i*SPLIT_AND_MERGE_D+4]) > ((int)line[i*SPLIT_AND_MERGE_D+5]-j) ){
				 line[i*SPLIT_AND_MERGE_D+5] = j;
				 j=endOfLine+1;
			 }
			 else{
				 line[i*SPLIT_AND_MERGE_D+4]=j+1;
			 }
		 }
	 }
 }

 (*nump)=k;
 
}


/**
 * Filters unvalid measurements from the measurements.
 * 
 * @param r distance measurement.
 * @param a angle of measurement
 * @return number of valid measurements, -1 if failed.
 */
int scanFilterNoMalloc(float *r, float *a,int inputSize) {
	//int inputSize = 0;
	int i = 0, index = 0;

	if(r == NULL || a == NULL) {
		return -1;	
	}

	// Go through all measurements
	//inputSize = _msize(r)/sizeof(float);
	for(i = 0; i < inputSize; i++) {
		if(r[i] < LASER_MAX_DIST) {
			r[index] = r[i];
			a[index] = a[i];
			index++;
		}
	}
	
	return index;
}

/**
 * Filters unvalid measurements from the measurements and
 * converts the (distance, angle) => (x, y). 
 * 
 * @param r distance measurement as input, x-coordinate as output.
 * @param a angle of measurement as input, y-coordinate as output.
 * @param inputSize size of arrays given as parameter
 * @return number of valid measurements, -1 if failed.
 */
int scanFilter2XYNoMalloc(float *r, float *a, int inputSize) {
	int i = 0, index = 0;
	float tempr = 0;

	if(r == NULL || a == NULL) {
		return -1;	
	}

	// Go through all measurements
	for(i = 0; i < inputSize; i++) {
		if(r[i] < LASER_MAX_DIST) {
			tempr = r[i];
			r[index] = tempr * cos(a[i]);
			a[index] = tempr * sin(a[i]);
			index++;
		}
	}
	
	return index;
}


/**
* Sets the line values for basic line
*@param x1 start x-coordinate of the line
*@param y1 start x-coordinate of the line
*@param x2 end x-coordinate of the line
*@param y2 end x-coordinate of the line 
*@param NumberOfPoints Number of scan points that the line contains 
*@return TLine new line
**/
TLine setLine(float x1,float y1, float x2, float y2, int NumberOfPoints){
	TLine L;

	L.x1=x1;
	L.x2=x2;
	L.y1=y1;
	L.y2=y2;
	
	L.numOfPoints = NumberOfPoints;
	
return L;
}

/**
* Calculate lenght of the basic line
*@param l line
*@return float The lenght of the line
*/
float lineLenght(TLine l){
	return sqrt((l.x1-l.x2) * (l.x1-l.x2) + (l.y1-l.y2) * (l.y1-l.y2));
}

/**
* Returns the angle of line 0..PI
*@param l 
*@return angle of line
*/
float lineAngle(TLine l){
	float dx, dy;
	float angle;

	dx = l.x1 - l.x2;
	dy = l.y1 - l.y2;
	angle = (float)atan2(dy, dx);
	if(angle<0) angle+=(float)PI;
return angle;
}

/**
* Calculates the distance of the point (x,y) from line L
* @return distance if succesful, else -1
*/
float pointDistanceFromLine(float x, float y, TLine L){
	if(sqrt(L.p.a*L.p.a + L.p.b*L.p.b)>0)
		return fabs( L.p.a * x + L.p.b * y + L.p.c )/sqrt(L.p.a*L.p.a + L.p.b*L.p.b);
	else return -1;
}	

/** 
* Returns the  angle difference between two lines
*
**/
float angleDiff(TLine l1, TLine l2){
	float dx1, dy1;
	float dx2, dy2;


	dx1 = l1.x1 - l1.x2;
	dy1 = l1.y1 - l1.y2;
	
	dx2 = l2.x1 - l2.x2;
	dy2 = l2.y1 - l2.y2;

	return acos(fabs (dx1*dx2+dy1*dy2) / ( sqrt(dx1*dx1+dy1*dy1) * sqrt(dx2*dx2+dy2*dy2) ) );
}

float shortestDistBetweenLineEndPoints(TLine L1,TLine L2){
	float l11,l12,l21,l22;
	
	l11 = (L1.x1-L2.x1)*(L1.x1-L2.x1) + (L1.y1-L2.y1)*(L1.y1-L2.y1);
	l12 = (L1.x1-L2.x2)*(L1.x1-L2.x2) + (L1.y1-L2.y2)*(L1.y1-L2.y2);
	l21 = (L1.x2-L2.x1)*(L1.x2-L2.x1) + (L1.y2-L2.y1)*(L1.y2-L2.y1);
	l22 = (L1.x2-L2.x2)*(L1.x2-L2.x2) + (L1.y2-L2.y2)*(L1.x2-L2.x2);

	return ___min(___min(l11,l12),___min(l21,22));
}


/**
* Convert line(end point, start point) to a * x + b * y + c = 0 form
*@return TLineParam line parameter structure
*/ 
TLineParam convertLine2abc(TLine L){
	TLineParam LP;

	LP.a = L.y2 - L.y1;
	LP.b = L.x1 - L.x2;
	LP.c = (-LP.a) * (L.x1) - (LP.b) * (L.y1);

return LP;
}

/**
* Rotates line around Pos
* @param L line to be rotated
* @param P Coordinate around which the rotation is done
* @param alpha rotation angle
*@return rotated line
*/

TLine rotateLine(TLine L,TPose P, float alpha){
	TLine RL;
	float cos_alpha = cos(alpha);
	float sin_alpha = sin(alpha);
	
	RL.numOfPoints = L.numOfPoints;

	RL.x1 = cos_alpha * (L.x1 - P.x) - sin_alpha * (L.y1 - P.y) + P.x;
	RL.y1 = sin_alpha * (L.x1 - P.x) + cos_alpha * (L.y1 - P.y) + P.y; 
 
	RL.x2 = cos_alpha * (L.x2 - P.x) - sin_alpha * (L.y2 - P.y) + P.x;
	RL.y2 = sin_alpha * (L.x2 - P.x) + cos_alpha * (L.y2 - P.y) + P.y;

	return RL;
}

/**
* int isInLine(double x0,double y0,TLine Line)
* checks if coordinate x0, y0 belongs to the line Line
* Returns 1 if it does 0 if not
*/
int isInLine(float x0,float y0,TLine Line){
	float mx1,mx2,my1,my2;
	float L,AC,AB;
	
	mx2=Line.x1; 
	my2=Line.y1; 
	mx1=Line.x2; 
	my1=Line.y2; 
				
	L=( (mx2-mx1)*(mx2-mx1) + (my2-my1)*(my2-my1) );
	AC=( (x0-mx2)*(x0-mx2) + (y0-my2)*(y0-my2) );
	AB=( (x0-mx1)*(x0-mx1) + (y0-my1)*(y0-my1) );
	
	if(AC<=L && AB<=L ) return 1; // yes it is in line
	else 
		return 0;
}


/**
* Calculates point (coordinate) from the TLine line, which is closest 
* to the TPose pos
*
* returns the TPose. OBS: the TPose heading value is not used!
**/  				
TPose shortestPointFromLine(TPose pos, TLine line){
	TPose point;
	float k,b;
	
	if(fabs(line.p.b)> 0.00001 && fabs(line.p.a)>0.00001){
		k = line.p.b/line.p.a; //Paitsi jos b=0
		b = pos.y - k * pos.x;
		
		point.x = -(b*line.p.b+line.p.c)/(line.p.a+line.p.b*k);
		point.y = k*point.x+b;
	}
	else if( fabs(line.p.b)  < 0.00001){
		point.x = -line.p.c/line.p.a;
		point.y = pos.y;
	}
	else{
		point.x = pos.x;
		point.y = -line.p.c/line.p.b;
	}
	point.a = 0;
	return point;
}


int roundi(float v){
	
	if( (v-floor(v)) > 0.5) return (int)floor(v)+1;
		else
			return (int)floor (v);
}

void calculateAngleHistogram(TLineVector L, float hist[181]){
	int i;
	float angle;

	for(i=0;i<181;i++) hist[i]=0;

	for(i=0;i<L.NumOfLines;i++){
		angle = lineAngle(L.Lines[i]);
	
		if(angle>PI) angle -= (float)PI;
		
		hist[roundi(angle*180.0/PI)] += L.Lines[i].numOfPoints;
		
	}
}

/**
* Rotates whole line vector around Pos
*@param LV Vector to be rotated
*@param Pos Coordinate around to be rotated
*@param alpha rotation angle
*@return rotated line vector
*/

TLineVector rotateLineVector(TLineVector LV, TPose P, float alpha){
	TLineVector RLV;
	int i;

	RLV = newLineVector(LV.NumOfLines);
	RLV.NumOfLines = LV.NumOfLines;

	for(i=0;i<LV.NumOfLines;i++)
		RLV.Lines[i] = rotateLine(LV.Lines[i],P,alpha);
	
	return RLV;
}

/**
* Calculates the weighted sum of angle differencies between 
* line pairs. 
*/
float estimateAngleBetweenLinePairs(TLinePairVector LPV){
	int i;
	//	float sum=0;
	float weightSum=0;
	float rx,ry,ax,ay;
	float r1,a1,dd;
	float a_cos=0,a_sin=0;


	for(i=0;i<LPV.numOfPairs;i++){
		rx = -(LPV.Pairs[i].L1.x1 - LPV.Pairs[i].L1.x2);
		ry = -(LPV.Pairs[i].L1.y1 - LPV.Pairs[i].L1.y2);
		ax = -(LPV.Pairs[i].L2.x1 - LPV.Pairs[i].L2.x2);
		ay = -(LPV.Pairs[i].L2.y1 - LPV.Pairs[i].L2.y2);
		r1 = sqrt( rx*rx + ry*ry);
		a1 = sqrt( ax*ax + ay*ay);
		dd = r1 * a1;

		a_cos += LPV.Pairs[i].weight * LPV.Pairs[i].weight * ( rx * ax + ry * ay)/dd;
		a_sin += LPV.Pairs[i].weight * LPV.Pairs[i].weight * ( ax*ry - ay*rx ) / dd;
		weightSum += LPV.Pairs[i].weight  * LPV.Pairs[i].weight;
	}
	a_cos = a_cos / weightSum;
	a_sin = a_sin / weightSum;

	return atan2(a_sin,a_cos);
}
/**
* Calculate The Mirek-correlation value for two lines
* The closer the value is to zero, the better is the correlation
*@copyright Mirek
**/

float calculateMirekCorrelationValue(TLine l1,TLine l2){
#define ANGLE_REJECTION_LIMIT 0.4 // = 22.9 degrees
	
	float a;
	float b;
	float c;
	float d;
	float x;
	float y;
	TLine lTmp;
	float adiff;
	float correlationValue;

	lTmp = setLine(l1.x1,l1.y1,l2.x1,l2.y1,0);
	a = lineLenght(lTmp);

	lTmp = setLine(l1.x1,l1.y1,l2.x2,l2.y2,0);
	b = lineLenght(lTmp);

	lTmp = setLine(l1.x2,l1.y2,l2.x1,l2.y1,0);
	c = lineLenght(lTmp);

	lTmp = setLine(l1.x2,l1.y2,l2.x2,l2.y2,0);
	d = lineLenght(lTmp);

	x = lineLenght(l2);
	y = lineLenght(l1);

	adiff =( angleDiff(l1,l2) );

	a = ___min(a,b);
	c = ___min(c,d);

	correlationValue = (a+c)/(x+y);
	
	if( fabs(adiff) > ANGLE_REJECTION_LIMIT) correlationValue += 1000; //Large number

	
return correlationValue;
}
/**
* Calculate The Mirek-correlation value for two lines
* The closer the value is to zero, the better is the correlation
*@copyright Mirek
**/

float calculateJariMirekCorrelationValue(TLine l1,TLine l2){
#define DISTANCE_REJECTION 1.0
#define TH 0.1
	
	float a;
	float b;
	float c;
	float d;
	float x;
	float y;
	TLine lTmp;
	float adiff;
	float correlationValue=0;
//	float alfa1,alfa2;
	int ind1,ind2;
	TPose p1,p2;
	float l;
	float criterias[4];
	int i,isOk=0;

	lTmp = setLine(l1.x1,l1.y1,l2.x1,l2.y1,0);
	a = lineLenght(lTmp);

	lTmp = setLine(l1.x1,l1.y1,l2.x2,l2.y2,0);
	b = lineLenght(lTmp);

	lTmp = setLine(l1.x2,l1.y2,l2.x1,l2.y1,0);
	c = lineLenght(lTmp);

	lTmp = setLine(l1.x2,l1.y2,l2.x2,l2.y2,0);
	d = lineLenght(lTmp);

	x = lineLenght(l2);
	y = lineLenght(l1);

	adiff =( angleDiff(l1,l2) );
	
	criterias[0] = fabs(a+b-x);
	criterias[1] = fabs(c+d-x);
	criterias[2] = fabs(a+c-y);
	criterias[3] = fabs(b+d-y);

	
	a = ___min(a,b);
	c = ___min(c,d);
	x = ___min(x,y);
	correlationValue = (a+c)/(x+y);
	correlationValue=0;
	
	for(i=0;i<4;i++){
		if((criterias[i])<TH){
			isOk++;
			correlationValue+=criterias[i];
		}
	}
	if(isOk>1){
		correlationValue=correlationValue/isOk;
	}
	else correlationValue+=1000;
	

	  


	//alfa1=lineAngle(l1);
	//alfa2=lineAngle(l2);
	//adiff = ___max(alfa1,alfa2)-___min(alfa1,alfa2);
	
	if( fabs(adiff) > ANGLE_REJECTION_LIMIT_JARI) {
		//mexPrintf("ADIF: %.2lf\n",adiff);
		correlationValue += 1000.0; //Large number
	}
	
	// If still valid pair, try to merge and test angles between 
	// merged line and original lines
	if(correlationValue < MAX_CORRELATION_JARI){
		lTmp=mergeTwoLines(l1,l2, ind1,ind2);
		adiff = fabs( angleDiff(lTmp,l1) );
		
		if(adiff < ANGLE_REJECTION_LIMIT_JARI / 2){
			adiff = fabs( angleDiff(lTmp,l2) );
			if(adiff > ANGLE_REJECTION_LIMIT_JARI / 2 ) correlationValue+=1000; 
		
		}
		else correlationValue+=1000;	
	}
	
	// if angles match:
	//check the distance of new line end point from original line 
	if(correlationValue < MAX_CORRELATION_JARI){
		//get end and start points
		if(ind1==1){
			p1.x=l1.x1; p1.y=l1.y1;
		}
		else p1.x=l1.x2; p1.y=l1.y2;
		if(ind2==1){
			p2.x=l2.x1; p2.y=l2.y1;
		}
		else p2.x=l2.x2; p2.y=l2.y2;
		
		l= ___max ( pointDistanceFromLine(p1.x,p1.y,l2), pointDistanceFromLine(p2.x,p2.y,l1));
		
		if(l > DISTANCE_REJECTION ) correlationValue+=1000;

	}


	
return correlationValue;
}

/**
* Returns the merges line so that the end and the start points are at maximum distance
**/
TLine mergeTwoLines(TLine L1, TLine L2, int& ind1, int& ind2){
	TLine retLine;
	float d[4];	
	int maxIndex = -1,i;
	float maxDist=0;

	d[0] = (L1.x1-L2.x1)*(L1.x1-L2.x1) + (L1.y1-L2.y1)*(L1.y1-L2.y1);
	d[1] = (L1.x1-L2.x2)*(L1.x1-L2.x2) + (L1.y1-L2.y2)*(L1.y1-L2.y2);
	d[2] = (L1.x2-L2.x1)*(L1.x2-L2.x1) + (L1.y2-L2.y1)*(L1.y2-L2.y1);
	d[3] = (L1.x2-L2.x2)*(L1.x2-L2.x2) + (L1.y2-L2.y2)*(L1.y2-L2.y2);
	
	for(i=0;i<4;i++){
		if(d[i]>=maxDist){
			maxDist=d[i];
			maxIndex=i;
		}
		
	}

//	if (maxIndex<0) mexPrintf("SOMETHING WRONG IN MERGING\n");

	switch(maxIndex){
	case 0:
		retLine = setLine(L1.x1,L1.y1,L2.x1,L2.y1,___max(L1.numOfPoints ,L2.numOfPoints));
		ind1=1;
		ind2=1;
		break;
	case 1:
		retLine = setLine(L1.x1,L1.y1,L2.x2,L2.y2,___max(L1.numOfPoints ,L2.numOfPoints));
		ind1=1;
		ind2=2;
		break;
	case 2:
		retLine = setLine(L1.x2,L1.y2,L2.x1,L2.y1,___max(L1.numOfPoints ,L2.numOfPoints));
		ind1=2;
		ind2=1;
		break;
	case 3:
		retLine = setLine(L1.x2,L1.y2,L2.x2,L2.y2,___max(L1.numOfPoints ,L2.numOfPoints));
		ind1=2;
		ind2=2;
		break;
	}

	retLine.p = convertLine2abc(retLine);


return retLine;
}

/**
* Find line pair for line L from line vector LV, using Mirek Correlation value
* if no pair is found weight of the line pair is set to -1;
*@param L Line, which is compared to vector LV
*@param LV Line vector 
*@param index Returns the index of Line in Vector LV that had the best correlation (-1 if no correspoding lines found
*/ 
TLinePair findLinePairFromVector(TLine L, TLineVector LV, int& index){
#define MAX_CORRELATION 0.5

	TLinePair LP;
	int minIndex=-1;
	float minCorValue=10000.0;
	float corValue;
	int i;

	for(i=0;i<LV.NumOfLines;i++){
		corValue = calculateJariMirekCorrelationValue(L,LV.Lines[i]);
		
		if(corValue<minCorValue){
			minCorValue = corValue;
			minIndex = i;
		}
	}
	if(minIndex>=0 && minCorValue < MAX_CORRELATION){
		LP.L1 = L;
		LP.L2 = LV.Lines[minIndex];
		LP.weight = ___min( L.numOfPoints , LV.Lines[minIndex].numOfPoints );
		index = minIndex;
		fprintf(stderr,"PAIR FOUND: %lf minCorValue\n",minCorValue);
	}
	else{
		fprintf(stderr,"No corresponding lines were found!!\n");
		LP.weight = -1;
		index = -1;
	}
	LP.corValue = minCorValue;

	return LP;
}
/**
* Find line pair for line L from line vector LV, using Mirek Correlation value
* if no pair is found weight of the line pair is set to -1;
*@param L Line, which is compared to vector LV
*@param LV Line vector 
*@param index Returns the index of Line in Vector LV that had the best correlation (-1 if no correspoding lines found
*/ 
TLinePair findLinePairFromMapVector(TLine L, TLineVector LV, int& index){


	TLinePair LP;
	int minIndex=-1;
	float minCorValue=1000000.0;
	float corValue;
	int i;

	for(i=0;i<LV.NumOfLines;i++){
		corValue = calculateJariMirekCorrelationValue(L,LV.Lines[i]);
		
		if(corValue<minCorValue){
			minCorValue = corValue;
			minIndex = i;
		}
	}
	if(minIndex>=0 && minCorValue < MAX_CORRELATION_JARI){
		LP.L1 = L;
		LP.L2 = LV.Lines[minIndex];
		LP.weight = ___min( L.numOfPoints , LV.Lines[minIndex].numOfPoints );
		index = minIndex;
		fprintf(stderr,"PAIR FOUND: %lf minCorValue\n",minCorValue);
		//mexPrintf("CorValue: %.2lf\n",minCorValue);
	}
	else{
		fprintf(stderr,"No corresponding lines were found!!\n");
		LP.weight = -1;
		index = -1;
	}
	LP.corValue = minCorValue;

	return LP;
}
/**
* Finds line pairs from two given line vectors. Compares each line in LV1 to LV2.
* Chooses the best correspondance between line in LV1 and LV2. Removes the found line
* from LV2 and repeats until the LV2 is empty or LV1 is checked.
*
*@param LV1 line vector to be matched against LV2
*@param LV2 matching vector (reference scan)
*@return TLinePairVector Vector of line found line pairs
*/
TLinePairVector findLinePairVectorFromLineVectors(TLineVector LV1, TLineVector LV2){
	int i,j;
	int index;
	TLinePair LP;
	TLinePairVector LPV;
	int maxNumOfPairs;
	//	int numOfPairs=0;

	maxNumOfPairs = ___min(LV1.NumOfLines,LV2.NumOfLines);
	
	//fprintf(stderr,"NUM OF PAIRS: %d ",maxNumOfPairs);
	
	LPV = newLinePairVector(maxNumOfPairs);
	
	for(i=0; i<LV1.NumOfLines;i++){
	
		if(LV2.NumOfLines>0){
			
			LP = findLinePairFromVector(LV1.Lines[i],LV2,index);
			
			if(LP.weight!= -1){
				LPV.Pairs[LPV.numOfPairs]=LP;
				LPV.numOfPairs++;
				//fprintf(stderr,"Pair::%d:: ",i); 
				//printLine(LP.L1);
				//printLine(LP.L2);
				
				for(j=index;j<LV2.NumOfLines-1;j++)	LV2.Lines[j]=LV2.Lines[j+1];
				LV2.NumOfLines--;
			}
		}
		else{
			fprintf(stderr,"running out of lines in LV2\n");
		}
	}

//	for(i=0;i<LPV.numOfPairs;i++){
//		fprintf(stderr,"PAIRS::%d:: ",i); 
//		printLine(LPV.Pairs[i].L1);
//		printLine(LPV.Pairs[i].L2);
//	}

return LPV;
}

/**
* Finds line pairs from two given line vectors. Compares each line in LV1 to LV2.
* Chooses the best correspondance between line in LV1 and LV2. Removes the found line
* from LV2 and repeats until the LV2 is empty or LV1 is checked.
*
*@param LV1 line vector to be matched against LV2
*@param LV2 matching vector (reference scan)
*@return TLinePairVector Vector of line found line pairs
*/
TLinePairVector findLinePairVectorFromLineVectorsNxN(TLineVector LV1, TLineVector LV2){
	int i,kk;
	int index;
	TLinePair LP;
	TLinePairVector LPV; //, LPV_tmp;
	int maxNumOfPairs;
	//	int numOfPairs=0;
	int* indVector;
	//int* indTmpVector;
	//float minC;
	int minI;
	int sameIndex;

	maxNumOfPairs = ___min(LV1.NumOfLines,LV2.NumOfLines);
	
	indVector = (int *) malloc(maxNumOfPairs * sizeof(int));
	indVector[0]=-1;
	//indTmpVector = (int *) malloc(maxNumOfPairs * sizeof(int));
	//fprintf(stderr,"NUM OF PAIRS: %d ",maxNumOfPairs);
	
	//LPV_tmp = newLinePairVector(maxNumOfPairs);
	LPV = newLinePairVector(maxNumOfPairs);
	
	


	for(i=0; i<LV1.NumOfLines;i++){
		
		if(LV2.NumOfLines>0){
			
			LP = findLinePairFromVector(LV1.Lines[i],LV2,index);
			
			if(LP.weight!= -1){ //Pair found
				sameIndex=-1;
				
				for(kk=0;kk<LPV.numOfPairs;kk++){ 
					if(indVector[kk] == index) sameIndex=kk;// there allredy exists pair for this index
				}	
				
				if(sameIndex !=-1){	
					if(LPV.Pairs[sameIndex].corValue<LP.corValue) minI = indVector[sameIndex]; 
					//original was better - do nothing
					else{
						// replace original with current one
						LPV.Pairs[sameIndex]=LP;
					}
				}
				else{
					// The pair was unique... add to the pair vector
					LPV.Pairs[LPV.numOfPairs]=LP;
					indVector[LPV.numOfPairs] = index;
					LPV.numOfPairs++;
				}
			}
			
		}
		else{
			fprintf(stderr,"running out of lines in LV2\n");
		}
	}



	free(indVector);
	

return LPV;
}



/**
* Allocates memory for NumOfLines Lines   
* and number of lines is set to 0, size is set to NumOfLines
**/
TLineVector newLineVector(int NumOfLines){
	TLineVector Lines;
	
	if(NumOfLines <= 0){
	  fprintf(stderr,"newLineVector:: Invalid number of lines, %d\n",NumOfLines);
	  Lines.NumOfLines = 0;
	  Lines.size = 0;
	  Lines.Lines = NULL;
	  return Lines;
	} 


	//Lines.Lines= (float *) malloc(4 * NumOfLines * sizeof(float));
	//Lines.NumOfPoints = (int *) malloc(NumOfLines * sizeof(int));
	Lines.Lines = (TLine *) malloc(NumOfLines * sizeof(TLine));
	Lines.NumOfLines=0;
	Lines.size = NumOfLines;
	return Lines;
}

TMapLineVector newMapLineVector(int NumOfLines){
	TMapLineVector V;
	int i;
	
	V.map = newLineVector(NumOfLines);
	V.timesMerged = (int *) malloc(NumOfLines * sizeof(int));

	for(i=0;i<NumOfLines;i++) V.timesMerged[i]=0;

return V;
}

/**
* Allocates memory for NumOfLines line pairs   
* and number of lines is set to zero, size is set to numOfPairs
**/

TLinePairVector newLinePairVector(int numOfPairs){
	TLinePairVector LPV;
	int i;

	LPV.Pairs = (TLinePair *) malloc(numOfPairs * sizeof(TLinePair));
	LPV.numOfPairs=0;
	LPV.size=numOfPairs;

	for(i=0;i<numOfPairs;i++){
		LPV.Pairs[i].corValue=1000;
		LPV.Pairs[i].weight = 0;
	}
	return LPV;
}

/**
* Frees allocated memory
**/
void freeLineVector(TLineVector Lines){
	
    if(Lines.Lines) free(Lines.Lines);
	//free(Lines.NumOfPoints);
}

void freeLinePairVector(TLinePairVector LPV){
	if(LPV.Pairs) free(LPV.Pairs);
}



/**
* Adds one line to the vector
*
* Line: Line to be included
* index: index where line is stored
**/
void addLine2Vector(TLineVector& Lines, TLine Line,int index){

	if(index>=0 && index<Lines.size){
		Lines.Lines[index] = Line; 
	}
	Lines.NumOfLines++;
}

/**
* Returns a single line from line vector
*
*@param Lines Vector of lines where the line is aquired 
*@param Index Index to line which is aquired
**/
TLine getLineFromVector(TLineVector Lines, int index){
	TLine line;

	if(index>=0 && index < Lines.size){
		line = Lines.Lines[index];
	}
	else{
		fprintf(stderr,"Incorrect line vector index!\n");
		line.x1=0;
		line.x2=0;
		line.y1=0;
		line.y2=0;
	}
	return line;
}

void printLine(TLine L){
	fprintf(stderr,"X1:%lf Y1:%lf X2:%lf Y2:%lf Points:%d\n",L.x1, L.y1, L.x2, L.y2, L.numOfPoints); 
}


void printLineVector(TLineVector LV){
	int i;

	for(i=0;i<LV.NumOfLines;i++){
		fprintf(stderr,"%d: ",i);
		printLine(LV.Lines[i]);
	}
}
/**
* Saves the linemap into the file 
* @param LV			The linevector
* @param *filename	the file to save to
*/
void saveLineVector(TLineVector LV, const char *filename){
	FILE *f;
	int i;

	f = fopen(filename, "wt");
	
	for(i=0;i<LV.NumOfLines;i++){
		fprintf(f,"%.3f ",LV.Lines[i].x1);
		fprintf(f,"%.3f ",LV.Lines[i].y1);
		fprintf(f,"%.3f ",LV.Lines[i].x2);
		fprintf(f,"%.3f\n",LV.Lines[i].y2);
	}
	
	fclose(f);
}

/**
 * Uses Least Squares Method to fit line into given points. 
 * Steps:
 * 1. Calculate average for points in x and y-direction. 
 * 2. Calculate covariance matrix for zero averaged points.
 * 3. Find eigenvector ([a b]) for covariance matrix, it tells
 *    the direction of maximum variance.
 * 4. Calculate projection for average(calculated in 1.) in the direction
 *    of eigenvector(calculated in 3.). This is the cross-section point
 *    with y-axis (C).
 * 5. Wanted line is: b*y + a*x + c = 0
 *
 * For more info about the angle calculation:
 * http://lib-www.lanl.gov/numerical/bookcpdf/c11-1.pdf
 *
 * @param x point x-coordinates
 * @param y point y-coordinates
 * @param size size of x and y-coordinate arrays.
 * @param line fitted line parameters.
 * @return 0 if success, else failed
 */
int lsq_estimate(float *x, float *y, int size, TLineParam *line) {
	float avex = 0, avey = 0;   // Coordinate average's
	float covxx = 0, covyy = 0, covxy = 0; // Covariance matrix
	float angle = 0;			// Angle of lsq line
	float a = 0, b = 0, c = 0;  // Line parameters
	float temp = 0;
	int i = 0;

	// Check parameters
	if(x == NULL || y == NULL || line == NULL || size <= 0)
		return -1;
	
	// Average for coordinates
	for(i = 0; size > i; i++) {
		avex += (float)x[i];
		avey += (float)y[i];
	}
	avex = avex/i;
	avey = avey/i;

	// Calculate covariance matrix [covxx, covxy; covyx, covyy],
	// matrix is build for zero average coordinate values.
	for(i = 0; size > i; i++) {
		covxx += (float)((x[i] - avex)*(x[i] - avex));
		covyy += (float)((y[i] - avey)*(y[i] - avey));
		covxy += (float)((x[i] - avex)*(y[i] - avey)); // == covyx
	}

	// Calculate angle using formula (11.1.8)
	// 11.1 Jacobi Transformations of a Symmetric Matrix
	// http://lib-www.lanl.gov/numerical/bookcpdf/c11-1.pdf
	angle = (float)(0.5*atan2(2*covxy, covyy-covxx));
	
	// Change angle so that slope can be calculated directly from tangent
	if(angle > 0) 
		angle = (float)PI/2 - angle;    
	else 
		angle = -angle - (float)PI/2;

	// Eigen vector can be calculated from the angle == [a b]
	a = (float)tan(angle); // Let's choose b = 1 => a = kk, because kk = a/b
	b = 1;
	temp = (float)sqrt(a*a + b*b);	// Normalize => eigen vector
	a /= temp; 
	b /= temp;

	// Calculate the intersection point for the line.
    // We had the following equation for the points moved to origo:
    // a*(x-avex) - b*(y-avey) = 0
    // When we open it we get:
    // y = a/b*x + (b*avey-a*avex)/b
    // Which is recogniced as a form of line:
    // y = B*x + C
	c = (b*avey - a*avex)/b;

	// Save in the wanted form
	// a*x + b*y + c = 0
	line->a = -a;
	line->b = b;
	line->c = -c*b;

	// Save number of points used
	line->numOfPoints = size;

	return 0;
}

/**
 * Uses Least Squares Method to fit line into given points. 
 * Steps:
 * 1. Calculate average for points in x and y-direction. 
 * 2. Calculate covariance matrix for zero averaged points.
 * 3. Find eigenvector ([a b]) for covariance matrix, it tells
 *    the direction of maximum variance.
 * 4. Calculate projection for average(calculated in 1.) in the direction
 *    of eigenvector(calculated in 3.). This is the cross-section point
 *    with y-axis (C).
 * 5. Wanted line is: b*y + a*x + c = 0
 * 6. Calculate nearest points in the line for first and last point.
 *
 * For more info about the angle calculation:
 * http://lib-www.lanl.gov/numerical/bookcpdf/c11-1.pdf
 *
 * @param x point x-coordinates
 * @param y point y-coordinates
 * @param size size of x and y-coordinate arrays.
 * @param endstart line estimated
 * @return 0 if success, else failed
 */
int lsq_estimate_points(float *x, float *y, int size, TLine *endstart) {
	TLineParam line;
	float d = 0;
	float temp_angle = 0;

	// Check parameters
	if(x == NULL || y == NULL || endstart == NULL || size <= 1)
		return -1;
	
	//////////////debug(DEBUGS, "LSQ-estimate[%d]: %.2f %.2f -> %.2f %.2f", size, x[0], y[0], x[size-1], y[size-1]);
	// Approximate the line for equatoin fro the points
	if (lsq_estimate(x, y, size, &line) != 0) return -1;

	// save the line parameters
	endstart->p.a = line.a;
	endstart->p.b = line.b;
	endstart->p.c = line.c;
	endstart->p.numOfPoints = line.numOfPoints;

	// Last point distance to the calculated line
	if(line.b == 0) line.b = (float)0.00001; // we don't want to divide with zero
	
	// start point
	d = (y[0] - (-line.a/line.b * x[0] - line.c/line.b));
	temp_angle = atan(-line.a/line.b);
	d = d*cos(temp_angle);
	endstart->x1 = (float)(x[0] + temp_angle/fabs(temp_angle)*d*sin(fabs(temp_angle)));
	endstart->y1 = (y[0] - d*cos(temp_angle));
	
	// end point
	d = y[size-1] - (-line.a/line.b * x[size-1] - line.c/line.b);
	d = d*cos(temp_angle);
	endstart->x2 = (float)(x[size-1] + temp_angle/fabs(temp_angle)*d*sin(fabs(temp_angle)));
	endstart->y2 = (float)(y[size-1] - d*cos(temp_angle));

	// Number of points
	endstart->numOfPoints = size-1;

	return 0;
}

/**
 * Convert laser scan to lines. Lines are formed with following steps.
 * 1. scan measurements are filtered to remove invalid scans
 * 2. split and merge algorithm collects the points into groups
 * 3. LSQ method fits a line to these points
 * 4. Nearest points of ending and starting point is found from the line.
 * 
 * @param scan_dists laser scan distance-array of size 361.
 * @param linev extracted lines are saved in here.
 * @param minimum_number_of_points the number of points that have
 *        to be found from line so that it is classified as line.
 * @return 0 if all ok, else -1
 */
int scans2FittedLines(float *scan_dists, TLineVector *linev, int minimum_number_of_points) {
	static float scan[361];		// distances of the scans
	static float scanA[361];       // angles used for scan
	static float scanLines[SPLIT_AND_MERGE_D*181]; // This is where the extracted angles are saved, 
									// no more than 181 angles can be found, ever.
	int scanLinesN = 0;				// Number of extracted lines.
	int scanN = 0;                  // Number of scans after filtering
	TLine tmpLine;					// Line is saved here before final installation
	int index = 0;                  // Which scan is now processed
	int i = 0, k = 0;

	// Check paramters
	if(scan == NULL || linev == NULL) {
		//fprintf(stderr, "scans2FittedLines: invalid parameters given.\n");
		return -1;
	}

	// Angles are initialized for normal scan
	for(i = 0; 361 > i; i++) {
		// Not wanted angles are marked with negative distance values
		if(scan_dists[i] >= 0) {
			scanA[index] = 0.5*i/180*PI;
			scan[index]  = scan_dists[i];
			index++;
		}
	}
	
	// Filter measurements, i.e. remove invalid values
	scanN = scanFilter2XYNoMalloc(scan, scanA, index);

	// Convert scan to lines
	split_and_merge(scan, scanA, scanLines, &scanLinesN, scanN, (float)0.1);
	//split_and_mergeAngle(scan, scanA, scanLines, &scanLinesN, scanN, 0.1);
	if(scanLinesN <= 0 || scanLinesN > 181) {
//		fprintf(stderr, "scans2FittedLines: invalid number of lines extracted.\n");
		return -1;
	}
	
	// Create new lines for vectors
	*linev = newLineVector(scanLinesN);

	/* Debug, to see split and merge without fitting
	for(i = 0; scanLinesN > i; i++) {	
		tmpLine.x1 = scanLines[i*D];
		tmpLine.y1 = scanLines[i*D+1];
		tmpLine.x2 = scanLines[i*D+2];
		tmpLine.y2 = scanLines[i*D+3];
		tmpLine.numOfPoints = scanLines[i*D+5] - scanLines[i*D+4];
		addLine2Vector(*linev, tmpLine, i);
	}*/

	for(i = 0; scanLinesN > i; i++) {	

		//debug(DEBUGS, "x = %.2f y = %.2f num = %d", scanLines[i*D+2], scanLines[i*D+3], scanLines[i*D+5] - scanLines[i*D+4]);
	}

	// LSQ fit for end and start points
	for(i = 0; scanLinesN > i; i++) {
		// fit only lines containing enough points
		if((int)(scanLines[i*SPLIT_AND_MERGE_D+5]-scanLines[i*SPLIT_AND_MERGE_D+4] + 1) > minimum_number_of_points) {
			// LSQ fit the line and calculate nearest points from the from the line
			// corresponding to end and start point
			if(lsq_estimate_points(scan+(int)scanLines[i*SPLIT_AND_MERGE_D+4], scanA+(int)scanLines[i*SPLIT_AND_MERGE_D+4], 
				(int)(scanLines[i*SPLIT_AND_MERGE_D+5]-scanLines[i*SPLIT_AND_MERGE_D+4] + 1), &tmpLine) == 0) {
				// If fit was succesful, add it the vector for return
				addLine2Vector(*linev, tmpLine, k);
				k++;
			}
		}
	}

	return 0;
}


/**
 * Convert laser scan to lines. Lines are formed with following steps.
 * 1. scan measurements are filtered to remove invalid scans
 * 2. split and merge algorithm collects the points into groups
 * 3. LSQ method fits a line to these points
 * 4. Nearest points of ending and starting point is found from the line.
 * 
 * @param x x-coordinates of the laser scan (filtered)
 * @param y y-coordinates of the laser scan (filtered)
 * @param scanN number of points in the scan
 * @param linev extracted lines are saved in here.
 * @param minimum_number_of_points the number of points that have
 *        to be found from line so that it is classified as line.
 * @return 0 if all ok, else -1
 */
int xyScan2FittedLines(float *x,float *y, int scanN, TLineVector *linev, int minimum_number_of_points) {
	static float scanLines[SPLIT_AND_MERGE_D*181]; // This is where the extracted angles are saved, 
									// no more than 181 angles can be found, ever.
	int scanLinesN = 0;				// Number of extracted lines.
	TLine tmpLine;					// Line is saved here before final installation
	//	int index = 0;                  // Which scan is now processed
	int i = 0, k = 0;

	// Check paramters
	if(x == NULL || y==NULL || linev == NULL ) {
		fprintf(stderr, "scans2FittedLines: invalid parameters given.\n");
		return -1;
	}


	// Convert scan to lines
	split_and_merge(x, y, scanLines, &scanLinesN, scanN, (float)0.1);
	if(scanLinesN <= 0 || scanLinesN > 181) {
		fprintf(stderr, "scans2FittedLines: invalid number of lines extracted.\n");
		return -1;
	}

	// Create new lines for vectors
	*linev = newLineVector(scanLinesN);

	// LSQ fit for end and start points
	for(i = 0; scanLinesN > i; i++) {
		// fit only lines containing enough points
		if((int)(scanLines[i*SPLIT_AND_MERGE_D+5]-scanLines[i*SPLIT_AND_MERGE_D+4] + 1) > minimum_number_of_points) {
			// LSQ fit the line and calculate nearest points from the from the line
			// corresponding to end and start point
			if(lsq_estimate_points(x+(int)scanLines[i*SPLIT_AND_MERGE_D+4], y+(int)scanLines[i*SPLIT_AND_MERGE_D+4], 
				(int)(scanLines[i*SPLIT_AND_MERGE_D+5]-scanLines[i*SPLIT_AND_MERGE_D+4] + 1), &tmpLine) == 0) {
				// If fit was succesful, add it the vector for return
				addLine2Vector(*linev, tmpLine, k);
				k++;
			}
		}
	}

	return 0;
}

/**
 * Converts given distance to x-y coordinates
 *
 * @param R distance array, size is 361, if distance < 0 then
 * it is ignored.
 * @param x x-coordinate
 * @param y y-coordinate
 * @return number of coordinates, or -1 if fails
 */
int distance2xycoordinates(float *R, float *x, float *y) {
	int i = 0, ret = 0;

	// Check parameters
	if(R == NULL || x == NULL || y == NULL) {
		fprintf(stderr, "distance2xycoordinates: invalid parameters.");
		return -1;
	}

	// convert to xy points
	for(i = 0; 361 > i; i++) {
		if(R[i] >= 0) {
			x[ret] = (float)(cos(i*0.5/180.0*PI)*R[i]);
			y[ret] = (float)(sin(i*0.5/180.0*PI)*R[i]);
			ret++;
		}
	}

	return ret;
}

/**
 * Takes two coordinates in a global coordinate system and returns 
 * differential movement compared to first scan.
 * 
 * @param pos1global Global position of first scan, array size is 3.
 * @param pos2global Global position of second scan, array size is 3.
 * @param posdif Differential movement from pos1global reference, array size is 3.
 * @return 0 if all ok, else -1
 */
int absolut2differentialPose(float *pos1global, float *pos2global, float *posdif) {
	float fii = 0, alfa = 0; // Angle between coordinates(fii) and angle in pos1 coordinate system
	float distance = 0;      // Distance between the two coordinates

	// Check parameters 
	if(pos1global == NULL || pos2global == NULL || posdif == NULL) {
		fprintf(stderr, "absolut2differentialPose: invalid paramets given.");
		return -1;
	}

	// Angle between pos1global and pos2global
	fii = atan2(pos2global[1]-pos1global[1], pos2global[0]-pos1global[0]);
	// Angle in pos1global coordinate system towards pos2global
	alfa = fii - pos1global[2];
	// Distance between pos1global and pos2global
	distance = sqrt(pow(pos2global[1]-pos1global[1],2) + pow(pos2global[0]-pos1global[0],2));

	// Result
	posdif[0] = distance * cos(alfa);
	posdif[1] = distance * sin(alfa);
	posdif[2] = pos2global[2] - pos1global[2];

	// Angle difference can't be over pi
	if(posdif[2] > PI) {
		posdif[2] = posdif[2] - 2*PI;
	}else if(posdif[2] < -PI) {
		posdif[2] = posdif[2] + 2*PI;
	}

	return 0;
}

/**
* Veery specific function to convert the line map into the text string
* MAP %d %.2f %.2f %.2f %.2f ....
* The %d is the number of lines in the string
* %.2f %.2f %.2f %.2f .... part contains %d times x1 y1 x2 y2 - line endpoint coordinates 
* @param lv		The line vector
* @param bigBuf	Big enough buffer to fit all the text there
*/
void linevector2j2b2Server(TLineVector lv, char *bigBuf){
	int i;
	
	sprintf(bigBuf,"MAP %d ",lv.NumOfLines);

	for(i=0;i<lv.NumOfLines;i++){
		sprintf(bigBuf,"%.2f %.2f %.2f %.2f ",lv.Lines[i].x1,lv.Lines[i].y1,lv.Lines[i].x2,lv.Lines[i].y2);	
	}
}
/**
* Another j2b2 specific function to convert the line string into TLineVector
*/
bool handleJ2B2ServerLinevectorString(char *buf, TLineVector &lv){
  bool result = false;
  int num,n;
  int i;
  char *first;
  char *x1,*x2,*y1,*y2;
	
  assert(buf != NULL);

  // Read in the number of lines entry
  first = strtok(buf," ");
  
  if (first) {
    i = sscanf(first,"%d", &num);
    
    if(i!=1){
      fprintf(stderr,"%s: No info about number of lines!!!\n", __FUNCTION__);
    
    } else {
      fprintf(stderr,"%s: Expecting %d lines\r\n", __FUNCTION__, num);
      
      // Construct new linevector with 'num' lines
      lv = newLineVector(num);

      // Now, set the result to 'true'
      result = true;

      for(i=0; i<num && result; i++){
	x1 = strtok(NULL," ");      
	y1 = strtok(NULL," ");       
	x2 = strtok(NULL," ");
	y2 = strtok(NULL," ");
	
	if(x1 == NULL || x2 == NULL || y1 == NULL || y2 == NULL){
	  fprintf(stderr,"%s: Invalid Line description!!!\n", __FUNCTION__);
	  result = false;

	} else {
	  n = sscanf(x1,"%f",&lv.Lines[i].x1);
	  n += sscanf(y1,"%f",&lv.Lines[i].y1);
	  n += sscanf(x2,"%f",&lv.Lines[i].x2);
	  n += sscanf(y2,"%f",&lv.Lines[i].y2);
	  if (n != 4) {
	    fprintf(stderr,"%s: Invalid Line description!!!\n", __FUNCTION__);
	    result = false;
	  
	  } else {
	    lv.NumOfLines++;
	    fprintf(stderr,"%s: Added line: %d. %.2f, %.2f, %.2f, %.2f\r\n",
		    __FUNCTION__,
		    lv.NumOfLines, 
		    lv.Lines[i].x1, lv.Lines[i].y1, 
		    lv.Lines[i].x2, lv.Lines[i].y2);
	  }
	}
	
      }
    }
  }
  return result;
}
